#include "colorAndResponse.hpp"
#include <math.h>
#include<assert.h>
#include<stdio.h>
#include<mex.h>

/*add a dummy line, to test how svn works for branches*/

// the arg_max_map works in two ways, 
// 1. it indicates the arg_max_deformation of an image
//2 . it indicates if an image is in this shape cluster
void::colorAndResponse::computeColorResponse(float **color_a_maps,float **color_b_maps,float **color_l_maps, float *p_Mats, float *p_views, double *p_Sx, double *p_Sy, double *p_Zx, double *p_Zy, int *arg_max_map){
	float **score_maps;
	float px,py; // projected x and y;
	int i_pix;
	for(	int i_img = 0; i_img<n_image; ++i_img){
		// update image projection matrix;
		camera.updateCamera(&p_Mats[i_img*12],&p_views[i_img*3],p_Zx[i_img],p_Zy[i_img]);
                int deformation = arg_max_map[i_img];
                if (!camera.isVisible()||deformation<0) {
                        for( int i_pt = 0; i_pt<gridX.size(); ++i_pt){
				p_a_score_map[i_pt][i_img]=NEGMAX;
				p_b_score_map[i_pt][i_img]=NEGMAX;
			}
			continue;
		}

		/*retrive deformation*/		

		/*get responses*/
		vector<float> &my_grid_x = this->deformedX[deformation];
		vector<float> &my_grid_y = this->deformedY[deformation];

                for( int i_pt = 0; i_pt<my_grid_x.size(); ++i_pt){
			camera.planePoint2Image(my_grid_x[i_pt],my_grid_y[i_pt],py,px);
                        //mexPrintf("%f,%f,%f,%f",my_grid_x[i_pt],my_grid_y[i_pt],py,px);
			int r_px =floor( px + 0.5f*p_Sx[i_img]+1 + 0.5f);
			int r_py =floor( py + 0.5f*p_Sy[i_img]+1 + 0.5f);
			if( r_px<0 || r_px>=p_Sx[i_img] || r_py<0 || r_py>=p_Sy[i_img]){
				p_a_score_map[i_pt][i_img]=NEGMAX;
				p_b_score_map[i_pt][i_img]=NEGMAX;
				p_l_score_map[i_pt][i_img]=NEGMAX;
			}else{
				i_pix = (r_px-1)+(r_py-1)*p_Sx[i_img];
				p_a_score_map[i_pt][i_img]=color_a_maps[i_img][i_pix];
				p_b_score_map[i_pt][i_img]=color_b_maps[i_img][i_pix];
				p_l_score_map[i_pt][i_img]=color_l_maps[i_img][i_pix];
			}
		}			
	}
}


void colorAndResponse::initializeGrids(int w, int h, int separation,int n_orient, int n_image){
	this->width = w;
	this->height = h;
	this->n_image = n_image;



	int cx = h/2;
	int cy = w/2;

	float lx = height/6.0f;
	float ly = width/6.0f;
	float ux = lx*5;
	float uy = ly*5;

	for(float x = lx; x<ux; x+=separation){
		for(float y = ly; y<uy; y+=separation){
			this->gridX.push_back(x);
			this->gridY.push_back(y);
		}
	}

	/* then push deformed versions;*/
	int ddx = (int)(height * 0.1f);
	int ddy = (int)(width * 0.1f);
	int step_x = (int)MAX(ddx,1);
	int step_y = (int)MAX(ddy,1);
	int max_rotation = 1;


	float x,y;// transformed x,y.
	vector<float> deform_x,deform_y;
	deform_x.resize(gridX.size()); deform_y.resize(gridY.size());
	float scale = 1.0f;
	//for( float scale = 0.8f; scale <1.21f; scale = scale + 0.2f){
		for( int dx = -ddx; dx <=ddx; dx+=step_x){
			for( int dy = -ddy; dy<= ddy; dy+=step_y){
				for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
					float theta = d_O*PI/n_orient;
					float R_mat[4];
					R_mat[0]=cos(theta);  R_mat[2]=-sin(theta);
					R_mat[1] = sin(theta); R_mat[3]= cos(theta);

					for(int i_pt=0; i_pt<gridX.size(); ++i_pt){
						x = gridX[i_pt]+dx-cx;
						y = gridY[i_pt]+dy-cy;
						deform_x[i_pt]=(R_mat[0]*x*scale + R_mat[2]*y*scale +cx); // new position, not new deformation
						deform_y[i_pt]=(R_mat[1]*x*scale + R_mat[3]*y*scale +cy); // new position, not new deformation
					}

					this->deformedX.push_back(deform_x);
					this->deformedY.push_back(deform_y);
				}
			}
		}
	//}

	assert(gridX.size()==gridY.size());
	p_a_score_map = (float**)malloc(gridX.size()*sizeof(float*));
	p_b_score_map = (float**)malloc(gridX.size()*sizeof(float*));
	p_l_score_map = (float**)malloc(gridX.size()*sizeof(float*));
	for( int i_pt = 0; i_pt<gridX.size(); ++i_pt){
		p_a_score_map[i_pt]=(float*)malloc(n_image*sizeof(float));
		p_b_score_map[i_pt]=(float*)malloc(n_image*sizeof(float));
		p_l_score_map[i_pt]=(float*)malloc(n_image*sizeof(float));
	}

}


void colorAndResponse::release(){

	for( int i_pt = 0; i_pt<gridX.size(); ++i_pt){
		free(p_a_score_map[i_pt]);
		free(p_b_score_map[i_pt]);
		free(p_l_score_map[i_pt]);
	}

	free(p_a_score_map);
	free(p_b_score_map);
	free(p_l_score_map);
}

void colorAndResponse::fitGaussianModel(float th){
	int npt= gridX.size();
	gain_list.resize(npt);
	live_list.resize(npt);
	model_param_list.resize(npt);
	/* fit model for each pt, and record the IG into gain_list*/
        double mu_a,mu_b,mu_a_2,mu_b_2,mu_l,mu_l_2;
	for( int i_pt= 0; i_pt<npt; ++i_pt){
		int n_sample = 0;
		mu_a=0; mu_b=0; mu_l=0; 
		mu_a_2=0; mu_b_2=0; mu_l_2=0;
            for ( int i_img=0; i_img<n_image; ++i_img){
			float a =  p_a_score_map[i_pt][i_img];
			float b =  p_b_score_map[i_pt][i_img];
			float l = p_l_score_map[i_pt][i_img];
			if(a<-1e6 || b<-1e6|| l<-1e6)	continue;
			mu_a +=a;
            mu_b +=b;
			mu_l +=l;
			mu_a_2 += a*a;
			mu_b_2 += b*b;
			mu_l_2 += l*l;

			++n_sample;
		}
		mu_a = mu_a/n_sample;
		mu_b = mu_b/n_sample;
		mu_l = mu_l/n_sample;
		mu_a_2 = mu_a_2/n_sample;
		mu_b_2 = mu_b_2/n_sample;
		mu_l_2 = mu_l_2/n_sample;

		float var_a = mu_a_2 - mu_a*mu_a;
		float var_b = mu_b_2 - mu_b*mu_b;
		float var_l = mu_l_2 - mu_l*mu_l;
		gain_list[i_pt] = (var_a+var_b+var_l)/3;
        mexPrintf("%f,%f,%f, n_sampe: %d,",mu_a,mu_b,mu_l,n_sample);
		model_param_list[i_pt].resize(6);
		model_param_list[i_pt][0] = mu_a;
		model_param_list[i_pt][1] = mu_b;
		model_param_list[i_pt][2] = mu_l;
		model_param_list[i_pt][3] = var_a;
		model_param_list[i_pt][4] = var_b;
		model_param_list[i_pt][5]= var_l;
		/*prune by applying a threshold*/
		if( gain_list[i_pt]>th){
			live_list[i_pt]=false;
		}else{
			live_list[i_pt]=true;
		}

                mexPrintf("ipt: %d, gain %f , th %f. \n",i_pt,gain_list[i_pt],th);
	}
}

void colorAndResponse::fitHistogram(){
     int npt = gridX.size();
     n_bin = 16;
     float tmp_hist[51];
     int i_bin;
     for( int i_pt = 0; i_pt<npt; ++i_pt){
        int n_sample = 0; 
        for( i_bin = 0; i_bin<3*n_bin+3; ++i_bin){
			tmp_hist[i_bin]=1e-6;
		}
        for( int i_img = 0; i_img<n_image; ++i_img){
		float a = p_a_score_map[i_pt][i_img];
		float b = p_b_score_map[i_pt][i_img];
		float l = p_l_score_map[i_pt][i_img];
  		if(a<-1e6 ||b<-1e6 ||l < -1e6) continue;
       	i_bin = floor(a * n_bin);
		tmp_hist[i_bin]=tmp_hist[i_bin]+1;
        i_bin = floor(b * n_bin);
        tmp_hist[i_bin + n_bin + 1]= tmp_hist[i_bin + n_bin +1] + 1;
		i_bin = floor(l * n_bin);
        tmp_hist[i_bin + 2 * n_bin + 2]= tmp_hist[i_bin + 2*n_bin + 2] + 1; 	
        ++n_sample;
	}
    for ( i_bin = 0; i_bin< 3*n_bin + 3; ++i_bin){
	   tmp_hist[i_bin]/=n_sample;
	}
       
    tmp_hist[n_bin-1]+=tmp_hist[n_bin]; tmp_hist[n_bin]=tmp_hist[n_bin-1];
	tmp_hist[2*n_bin] +=tmp_hist[2*n_bin+1]; tmp_hist[2*n_bin+1]=tmp_hist[2*n_bin];
    tmp_hist[3*n_bin + 1] += tmp_hist[3*n_bin+2]; tmp_hist[3*n_bin+2]=tmp_hist[3*n_bin+1];
      
    /*copy the response back*/	
	std::vector<float> hist;
    hist.reserve((n_bin+1)*3);
	for(int i_bin =0; i_bin< 3*n_bin + 3; ++i_bin){
		hist.push_back(tmp_hist[i_bin]);
	}
	model_hist.push_back(hist);
    }
}
void colorAndResponse::outputInCanonicalView(const char *file_name){
	FILE *p_file;
	p_file=fopen(file_name,"w");
	if( p_file == NULL){
		printf("error:: can not create file %s!!! will return without output\n",file_name);
		return;
	}
        printf("file created %s.\n",file_name);

    fprintf(p_file,"%d,%d,\n",width,height);
	for( int i_pt=0; i_pt<gain_list.size(); ++i_pt){
		if(!live_list[i_pt]) continue;
		fprintf(p_file,"0,%f,%f,%f,%f,%f,%f,%f,%f",  gridX[i_pt],gridY[i_pt],model_param_list[i_pt][0],model_param_list[i_pt][1] ,model_param_list[i_pt][2] ,model_param_list[i_pt][3] ,model_param_list[i_pt][4],model_param_list[i_pt][5] );
        	for(int i_bin=0; i_bin<3*n_bin+3; ++i_bin){
		  fprintf(p_file,",%f",model_hist[i_pt][i_bin]); 
		}      
	 fprintf(p_file,"\n");       
         /*mexPrintf("IG: %f, position: %f,%f, param: %f,%f,%f,%f,%f\n", gain_list[i_pt], gridX[i_pt],gridY[i_pt],
																											model_param_list[i_pt][0],
																											model_param_list[i_pt][1] ,
																											model_param_list[i_pt][2] ,
																											model_param_list[i_pt][3] ,
                                                                                                                                                                                                                model_param_list[i_pt][4] );
 */
}
    
	fclose(p_file);
}
